/*	///////////////////////////////////////////////////////////////////////
		ModelOMatic -- Program for assessing the fit of lots of models
			Simon Whelan, Uppsala University

		ModelOMatic <data_file> <tree_file> <output_file> <Genetic_code; default=universal> <fast>

	Option definitions
	<data_file>		Any sequence data file in phylip or sequential format
	<tree_file>		The tree
	<output_file>	Output file
	<Genetic_code> 	a number between 0 and something
	<fast>			if fast is present it will only optimise the branches and alpha parameter for the first model of a data type and then fix for all subsequent
	/////////////////////////////////////////////////////////////////////// */

// Note: this version was sent to David T @ Thu 14 March 2013

#include "./ModelAssess.h"
#include <time.h>
#include <set>

#define CHECK_LNL_OUT 1
#define VERSION_NUMBER "1.0a"

#if FUNC_COUNTERS == 1
	extern int Matrix_Log_Counter, MakeQ_Log_Counter, MakePT_Log_Counter, LFunc_Log_Counter, SubLFunc_Log_Counter, SPR_Log_Counter;
#endif
vector <double> GetPWVar(CBaseModel *Model, vector <double> *CurrentPW);
double GetDistVar(CBaseModel *M, CData *D,int Seq1, int Seq2, double CurrentDist);
extern bool ALLOW_PREDICTLNL;

/////////////////////////////////////////////////////////////////////////////////
// Main tree estimation routine
SBestModel DoTreeEstimation(CBaseModel *M, CData *D, CTree *T);

// Pointer to likelihood function that changes depending on the type of calculation performed
vector <STabuTree> TabuTrees;
vector <double> PWDists;
CPhyloDat PhyDat;
int TABU_RADIUS = DEFAULT_TABU_RADIUS, EXIT_OBS, OptObs;
double PROB_RAN_SEQ_REM = DEFAULT_PROB_RAN_SEQ_REM;
bool AllowPreOpt = true, DoItFast = false, DoItTrim = false, ModelOut = false;
int TrimTree = 10;		// Number of sequences defaulted to by the DoItTrim option
void DoInstructions();
bool RunHadError = false;

bool WarningMulD;

// File specific global variables
int DebugOutput = 0;

int n_models=13;
string model_names[] = {"JTT","WAG","LG","DAY","mtREV","mtMam","mtArt","rtREV","cpREV","BLOSUM62","VT","HIVb","HIVw"};
double* smat[] = {(double*)dJTTVal,(double*)dWAGVal,(double*)dLGVal,(double*)dDAYVal,(double*)dmtREVVal,(double*)dmtMAMVal,(double*)dmtArtVal,(double*)drtREVVal,(double*)dcpREVVal,(double*)dBLOSUM62Val,(double*)dVTVal,(double*)dHIVbVal,(double*)dHIVwVal};
double* freq[] = {(double*)dJTTFreq,(double*)dWAGFreq,(double*)dLGFreq,(double*)dDAYFreq,(double*)dmtREVFreq,(double*)dmtMAMFreq,(double*)dmtArtFreq,(double*)drtREVFreq,(double*)dcpREVFreq,(double*)dBLOSUM62Freq,(double*)dVTFreq,(double*)dHIVbFreq,(double*)dHIVwFreq};

std::map <string,double**> aa_model_map;
std::set <string> codon_model_set;

int main(int argc, char *argv[])	{
	int i;
	vector <s_sequences> v_Sequences;
	string SeqInput = "outputname.fas";

	v_Sequences = ReadSequences(SeqInput);

	FOR(i,v_Sequences.size()) { cout << "\n" << v_Sequences[i].Name << "\t" << v_Sequences[i].Sequence; } cout << "\n\n";


	// Some general testing stuff for an initial pairHMM
	CPairHMM PairHMM(v_Sequences[0],v_Sequences[1],DNA_ABET);
	PairHMM.CreateModel(HKY,0.1);
//	PairHMM.OutModel();

/*
	cout << "\nPerforming Viterbi\n-----------------------------\n";

	PairHMM.DoAln();

	cout << "\nLikelihood OptPath: " << PairHMM.m_dViterbi_lnL;

	cout << "\nPerforming Forward\n-----------------------------\n";
	PairHMM.Forward();

	cout << "\nLikelihood AllPath: " << PairHMM.m_dForward_lnL;

	cout << "\nAnd the lnL: " << PairHMM.lnL();
*/
	cout << "\nApproxForward:	" << PairHMM.ApproxForward();

	cout << "\nOptimising evolutionary distance";
	PairHMM.m_bDoForwardlnL = true;
	DumbOptimiser(&PairHMM);

	cout << "\nModelOutput: ";
	PairHMM.OutModel();

	cout << "\nFinal likelihood estimate: " << PairHMM.Forward();
	PairHMM.CleanApprox();
	cout << "\nForward:      	" << PairHMM.Forward();

/*
	cout << "\nAnd the viterbi: ";
	PairHMM.DoAln();
	cout << "\nViterbi lnL" << PairHMM.m_dViterbi_lnL;

	// Output surfaces.
	cout << "\n--------- SURFACES --------------";
	PairHMM.OutputTimeSurface();
	cout << "\n------";
	PairHMM.OutputIndelRateSurface();
	cout << "\n------";
	PairHMM.OutputProbExtendSurface();
	cout << "\n------";
	PairHMM.OutputFractInsSurface();
	cout << "\n------";
	*/
	cout << "\nSuccessful exit\n";
	return 0;
}

// Reads a set of sequences into a simple structure
// Strong assumes sequences of the format
// <white_space {optional}>
// >SequenceName
// SEQUENCE
// SEQUENCE <continued>
// <white_space>
vector <s_sequences> ReadSequences(string seqin) {
	vector <s_sequences> ret_v_seq;
	vector <string> Toks;
	string store;
	string seq;
	s_sequences current_seq;

	// Open file
	FINOPEN(input,seqin.c_str());

	// Do the read
	store = GetDataLine(&input);		// Read to the first line
	while(!input.eof()) {
		// Loop for getting sequence
		// 1. Get the name
		Toks = Tokenise(store);
		if(Toks.size() > 2) { Error("Tokenised line {" + store + "} and have more than 2...\n"); }
		if(Toks[0][0] != '>') { Error("FASTA names must begin with '>'\n"); }
		current_seq.Name = find_and_replace(Toks[0],">","");
		// 2. Get the sequence0
		if(Toks.size() > 1) { 	// Assume the entire sequence is on the same line
			Toks[1] = find_and_replace(seq," ","");
			current_seq.Sequence = Toks[1];
		} else {				// The sequence follows over the next few lines
			seq = ""; store = "";
			getline(input,store);
			while(!input.eof() && store[0] != '>')  {
				if(store[0] != '\n' || store[0] != '\0' || store[0] != '#' || store[0] != ' ') { seq += store; }
				getline(input,store);
			}
			seq = find_and_replace(seq," ","");
			current_seq.Sequence = seq;
		}
		ret_v_seq.push_back(current_seq);
		if(input.eof()) { break; }
	}
	input.close();
	return ret_v_seq;
}

//////////////////////////////////////////////////////////////////////////////
// The pairHMM class: defines the whole model

// Constructor function. Reads a pair of sequences and an alphabet and populates the class with these data
CPairHMM::CPairHMM(s_sequences SeqA, s_sequences SeqB, string ABET) : CBaseModel(NULL,NULL) {
	int i,j;
	// Do sequence A
	m_sNameA = SeqA.Name;
	FOR(i,(int)SeqA.Sequence.size()) {
		FOR(j,(int) ABET.size()) { if(SeqA.Sequence[i] == ABET[j]) { m_viSeqA.push_back(j); break; } }
		if(j == (int) ABET.size()) { cout << "\nFound sequence <"<<SeqA.Name << "> at site["<< i<<"] = '" << SeqA.Sequence[i] << "', which is not a recognised value in the proposed alphabet: " << ABET << "\n\n"; exit(-1); }
	}
	// Do sequence B
	m_sNameB = SeqB.Name;
	FOR(i,(int)SeqB.Sequence.size()) {
		FOR(j,(int) ABET.size()) { if(SeqB.Sequence[i] == ABET[j]) { m_viSeqB.push_back(j); break; } }
		if(j == (int) ABET.size()) { cout << "\nFound sequence <"<<SeqB.Name << "> at site["<< i<<"] = '" << SeqB.Sequence[i] << "', which is not a recognised value in the proposed alphabet: " << ABET << "\n\n"; exit(-1); }
	}
	// Store the alphabet
	m_sABET = ABET;
	m_iChar = (int) ABET.size();
	// Some other parameters
	m_dViterbi_lnL = m_dForward_lnL = -BIG_NUMBER;
	m_bDoForwardlnL = true;
	// Create the parameters of the model
	m_dTime = new CQPar("t",m_iChar,0.15);
	m_dIndelRate = new CQPar("IndelRate",m_iChar,0.05);
	m_dFractionInsert = new CQPar("FractionIndel",m_iChar,0.5, true,1.0E-3,1-1.0E-3);
	m_dProbExtend = new CQPar("ProbExtend",m_iChar,0.25,true,1.0E-3,1-1.0E-3);
	// Get the frequencies associated with the model
	m_vdDataFreq.assign(4,0);
	FOR(i,(int)m_viSeqA.size()) { m_vdDataFreq[m_viSeqA[i]]+=1.0; }
	FOR(i,(int)m_viSeqB.size()) { m_vdDataFreq[m_viSeqB[i]]+=1.0; }
	m_vdDataFreq = NormaliseVector(m_vdDataFreq);
	// Create probability vectors
	m_vdMatchTo.assign(3, 0.0);
	m_vdInsTo.assign(3, 0.0);
	m_vdDelTo.assign(3, 0.0);
	// Space empty
	m_pProbMatchMat = NULL;
	m_pProbInsMat = NULL;
	m_pProbDelMat = NULL;
	m_pModel = NULL;
	m_iApproxSize = APPROX_STARTSIZE;
	m_bDoApprox = false;
}

// Destructor function
CPairHMM::~CPairHMM() {
	assert(m_pProbMatchMat == NULL);
	assert(m_pProbInsMat == NULL);
	assert(m_pProbDelMat == NULL);
	m_vdMatchTo.clear(); m_vdInsTo.clear(); m_vdDelTo.clear();
	delete m_pModel;
	delete m_dTime;
	delete m_dIndelRate;
	delete m_dFractionInsert;
	delete m_dProbExtend;
}

// Model creation function
// -- Still a work in progress. At the moment it's just an arbitrary JC type of model.
/*
enum EModel {	JC,FEL,K2P,HKY,REV,								// Standard nucleotide
				RY_model, COV_HKY,COV_REV,THMM_FULLDNA, HKYdG_THMM,	THMM_DNA, // Unusual nucleotide
				EQU,WAG,JTT,DAY,mtREV,							// Standard amino acid
				THMM_AA,WAGdG_THMM,								// Unusual amino acid
				Coevo_WAG,										// Coevolution models
				CodonM0,										// Codon
				UNKNOWN};
*/
bool CPairHMM::CreateModel(EModel ModChoice, double IndelRate) {
	int i;
	CJC * mJC = NULL;
	CK2P *mK2P = NULL;
	CFEL * mFEL = NULL;
	CHKY * mHKY = NULL;
	CREV * mREV = NULL;
	m_pDummyTree = new CTree("(1,2);",2);
	m_pDummyData = new CData(2,0,DNA);
	m_pDummyData->m_vFreq = m_vdDataFreq;

	switch(ModChoice) {
	case JC:
		mJC = new CJC(m_pDummyData,m_pDummyTree);
		m_pModel = mJC; mJC = NULL;
		break;
	case FEL:
		mFEL = new CFEL(m_pDummyData,m_pDummyTree);
		m_pModel = mFEL; mFEL= NULL;
		break;
	case K2P:
		mK2P= new CK2P(m_pDummyData,m_pDummyTree);
		m_pModel = mK2P; mK2P = NULL;
		break;
	case HKY:
		mHKY = new CHKY(m_pDummyData,m_pDummyTree);
		m_pModel = mHKY; mHKY = NULL;
		break;
	case REV:
		mREV = new CREV(m_pDummyData,m_pDummyTree);
		m_pModel = mREV; mREV = NULL;
		break;
	default: Error("\nModel not implemented yet...\n");

	}
	m_pModel->m_pTree->SetB(0,m_dTime->Val());
	m_pModel->PreparelnL(true);
	FOR(i,(int)m_pModel->m_vpProc.size()) { m_pModel->m_vpProc[0]->CreatePTMats(0); }

	// input checks
	assert(m_vdQMatModel.empty() && m_vdPTModel.empty());
	assert(IndelRate >= 0.0);
	if(IndelRate > 1.0) { cout << "\nWARNING: Indelrate = " << IndelRate << " above substitution rate. Expect weird results..."; }

	m_dIndelRate->SetVal(IndelRate,true);

	if(m_pModel->m_vpProc.size() != 1) { Error("The whole pairHMM thing has been set up for a single PT matrix at present..."); }
	// Guess parameters
	GuessParameters();
	// And return
	return true;		// Successful completion
}

// Function to guess the parameters of a model
// Current implementation is extremely basic. It builds a Viterbi alignment and estimate
void CPairHMM::GuessParameters() {
	int i, same = 0, total = 0;
	double dist = 0;
	Viterbi();
	// Check it's worked
	assert(m_viAln.size() == 2);
	// Compute difference
	FOR(i,(int)m_viAln[0].size()) {
		if(m_viAln[0][i] == -1 || m_viAln[1][i] == -1) { continue; } // Skip gaps
		if(m_viAln[0][i] != m_viAln[1][i]) { same++; }
		total ++;
	}
	dist = (double) same / (double) total;
	if(dist >= 0.75) { m_dTime->SetVal(MAX_TIME); } // Deal with infinities
	else { m_dTime->SetVal(min(-(3.0/4.0)*log(1-((4.0/3.0)*dist)),MAX_TIME),true); } // Otherwise Jukes and Cantor
	m_pModel->m_pTree->SetB(0,m_dTime->Val());
//	cout << "\nDone to here " << same << " / " << total << " and distance: " << m_dTime->Val(); ;
}

// Builds a model based on the Q matrix.
// -- Still a work in progress...
bool CPairHMM::BuildModel(bool DoLog)	{
	int i;
	double ProbExtend = m_dProbExtend->Val();
	// Create the model
	assert(m_pModel != NULL);

	// Get the PT matrices ready
	m_pModel->m_pTree->SetB(0,m_dTime->Val());
	m_pModel->PreparelnL(true);
	FOR(i,(int)m_pModel->m_vpProc.size()) { m_pModel->m_vpProc[0]->CreatePTMats(0); }

	// This bit assumes a single PT process
	m_vdPTModel = m_pModel->m_vpProc[0]->GetPT(0);
//	m_pModel->OutPT(cout,0,0);
	m_vdStateFreq = m_pModel->m_vpProc[0]->Eqm(0);

	// The matrix used for alignment needs to be  Pr(A)Pr(AB), as in how likelihoods between pairs of sequences are calculated
	FOR(i,(int)m_vdPTModel.size()) { m_vdPTModel[i] *= m_vdStateFreq[i%m_iChar]; }

	// Calculate probability of an Indel
	// Rate of each process is the Indel = IndelRate * branch length
	// Probability of event is 1 - E(no change) = 1 - exp(-RateIndel)

	m_dProbIndel = 1.0 - exp(-m_dIndelRate->Val() * m_dTime->Val());		// Defined through standard poisson process, but problem is probability can be greater than 1.0. Yuck!
	// Half of these events are insertions and half are deletions, so the individual rates are split in two
	m_dProbIndel *= m_dFractionInsert->Val();

	// Overwrite some values for fast simple testing
/*	FOR(i,4) { m_vdStateFreq[i] = 0.25; }
	double mat[] = { 	0.8, 0.05, 0.1, 0.05,
						0.05, 0.8, 0.05, 0.1,
						0.1, 0.05, 0.8, 0.05,
						0.05, 0.1, 0.05, 0.8};
	FOR(i,16) { m_vdPTModel[i] = mat[i] * m_vdStateFreq[i%4]; }
	m_dProbIndel = 0.05;
	MatOut(4,m_vdPTModel);
*/

	// Assign the transition probabilities between hidden states
	// Hidden model corresponds to a fragment process

	// The match state from M->M with no internal loop
	m_vdMatchTo[0] = 1.0 - (2* m_dProbIndel);
	m_vdMatchTo[1] = m_dProbIndel;
	m_vdMatchTo[2] = m_dProbIndel;

	/*
	// The match state with an internal loop of rate ProbExtend (Following Redelings/Suchard 2008)
	m_vdMatchTo[0] = m_dProbExtend + ( (1.0 - m_dProbExtend) * ( 1.0 - ( 2 * m_dProbIndel) ) );
	m_vdMatchTo[1] = (1.0 - m_dProbExtend) * m_dProbIndel;
	m_vdMatchTo[2] = (1.0 - m_dProbExtend) * m_dProbIndel;
	*/
	assert(fabs(1.0 - Sum(&m_vdMatchTo)) < FLT_EPSILON);
	m_vdInsTo[0] = (1.0 - ProbExtend) * (1.0 - (2*m_dProbIndel));
	m_vdInsTo[1] = ProbExtend + ((1.0 - ProbExtend) * m_dProbIndel);
	m_vdInsTo[2] = (1.0 - ProbExtend) * m_dProbIndel;
	assert(fabs(1.0 - Sum(&m_vdInsTo)) < FLT_EPSILON);
	m_vdDelTo[0] = (1.0 - ProbExtend) * (1.0 - (2*m_dProbIndel));
	m_vdDelTo[1] = (1.0 - ProbExtend) * m_dProbIndel;
	m_vdDelTo[2] = ProbExtend + ((1.0 - ProbExtend) * m_dProbIndel);
	assert(fabs(1.0 - Sum(&m_vdDelTo)) < FLT_EPSILON);




	// Some asserts to make sure the model is valid
	assert(m_iChar == (int)m_vdStateFreq.size());

	// Log things if required
	if(DoLog) {
		// For Viterbi all frequencies at the end of this should be logged
		FOR(i,m_vdPTModel.size()) { m_vdPTModel[i] = log(m_vdPTModel[i]); }
		FOR(i,m_iChar) { m_vdStateFreq[i] = log(m_vdStateFreq[i]); }
		// Log transition probabilities
		FOR(i,3) {
			if(m_vdMatchTo[i] < FLT_EPSILON) { m_vdMatchTo[i] = -BIG_NUMBER; }
			else { m_vdMatchTo[i] = log(m_vdMatchTo[i]); }
			if(m_vdInsTo[i] < FLT_EPSILON) { m_vdInsTo[i] = -BIG_NUMBER; }
			else { m_vdInsTo[i] = log(m_vdInsTo[i]); }
			if(m_vdDelTo[i] < FLT_EPSILON) { m_vdDelTo[i] = -BIG_NUMBER; }
			else { m_vdDelTo[i] = log(m_vdDelTo[i]); }
		}
	}
/*
	cout << "\nm_vdPTModel:   " << m_vdPTModel << " == " << Sum(&m_vdPTModel);
	cout << "\nm_vdStateFreq: " << m_vdStateFreq << " == " << Sum(&m_vdStateFreq);
	cout << "\nProbability of indel = " << m_dProbIndel;
	cout << "\nm_vdMatchTo:   " << m_vdMatchTo << " == " << Sum(&m_vdMatchTo);
	cout << "\nm_vdInsTo:     " << m_vdInsTo << " == " << Sum(&m_vdInsTo);
	cout << "\nm_vdDelTo:     " << m_vdDelTo << " == " << Sum(&m_vdDelTo);
*/
	return true;
}

// Model output
bool CPairHMM::OutModel(ostream &os) {
	int i;
	// Function checkers
	assert(m_vdPTModel.size() / m_iChar == m_iChar && !m_vdPTModel.empty());
//	assert(m_vdQMatModel.size() / m_iChar == m_iChar && !m_vdQMatModel.empty());
	// Output functions
	os << "--- PairHMM model details ---";
	os << "\nTime: " << *m_dTime;
	os << "\nModel: " << m_pModel->Name();
//	os << "\nQMatrix:";
//	MatOut(m_iChar,m_vdQMatModel,'\t',os);
	os << "\nIndelRate:   " << m_dIndelRate->Val();
	os << "\nFractionIns: " << m_dFractionInsert->Val();
	os << "\nProbExtend: " << m_dProbExtend->Val();
	os << "\nStateFreq:   " << m_vdStateFreq;
	os << "\nPT";
	MatOut(m_iChar,m_vdPTModel,'\t',os);
	os << "\nIndelProb:   " << m_dProbIndel;
	os << "\nTransition probabilities for HMM: [0] To Match; [1] To Insert; [2] To Delete";
	os << "\nFromMatch   " << m_vdMatchTo;
	os << "\nFromIns     " << m_vdInsTo;
	os << "\nFromDel     " << m_vdDelTo;
	return true;
}
///////////////////////////////////////////////////////
// Viterbi algorithm for pairHMM
// ---
// Returns two strings corresponding to the pairwise alignment for SeqA and SeqB
double CPairHMM::Viterbi(bool StorePath)	{
	int i,j,k;
	int sizeA = m_viSeqA.size()+1, sizeB = m_viSeqB.size()+1;
	double logL;
	if(!m_viAln.empty()) { m_viAln.clear(); }
	m_viAln.assign(2,vector <int>(0,0));


	// Check entry conditions
	assert(m_vdMatchMat.empty() && m_vdInsMat.empty() && m_vdDelMat.empty());	// All probability storage should be empty.
	assert(!m_viSeqA.empty() && !m_viSeqB.empty());
	// If storing traceback ensure it's ready
	if(StorePath) { if(m_viApproxPath.empty()) { Error("\nTrying to store path in CPairHMM::Viterbi... m_iApproxPath has not been initialised!\n"); } }

	// Create some space
	int TotalSize = (m_viSeqA.size() + 1) * (m_viSeqB.size() + 1);
	m_vdMatchMat.assign(TotalSize,-BIG_NUMBER);
	m_vdMatchMat[0] = 0.0;	// Standard starting condition to begin somewhere.
	m_vdInsMat.assign(TotalSize,-BIG_NUMBER);
	m_vdDelMat.assign(TotalSize,-BIG_NUMBER);
	// Build the model
	assert(BuildModel(true));
	// Clarify referencing
	// index i (rows) correspond to sequence A
	// index j (columns) correspond to sequence B
	// Initialise necessary edges
	// 1. Sequence A (inserts before match)
//	cout << "\n --- INITIAL ---";
	for(i=1;i<sizeA;i++) {
		m_vdInsMat[(i*sizeB)] = m_vdStateFreq[m_viSeqA[i-1]] + max(max(Match2Ins(i,0),Ins2Ins(i,0)),Del2Ins(i,0));
//		cout << "\n[i=" << i << " == " << (i*sizeB) <<"] = " << m_vdInsMat[(i*sizeB)];
	}
	// 2. Sequence B (deletes before match)
	for(j=1;j<sizeB;j++) {
		m_vdDelMat[j] = m_vdStateFreq[m_viSeqB[j-1]] + max(max(Match2Del(0,j),Ins2Del(0,j)),Del2Del(0,j));
//		cout << "\n[j=" << j << "] = " << m_vdDelMat[j];
	}

	// And now we're ready to go... Viterbi on
	for(i=1;i<sizeA;i++) {
		for(j=1;j<sizeB;j++) {
//			cout << "\n("<<i<<"," << j << ") == " << (sizeB*i)+j << "/" << TotalSize << flush;
			// Do Match states
			m_vdMatchMat[(i*sizeB)+j] = m_vdPTModel[(m_viSeqA[(i-1)]*m_iChar) + m_viSeqB[j-1]] + max(max(Match2Match(i,j),Ins2Match(i,j)),Del2Match(i,j));
//			cout << " M" << flush;
			// Do I states (outputs seqA)
			m_vdInsMat[(i*sizeB)+j] = m_vdStateFreq[m_viSeqA[i-1]] + max(max(Match2Ins(i,j),Ins2Ins(i,j)),Del2Ins(i,j));
//			cout << "I" << flush;
			// Do D states (outputs seqB)
			m_vdDelMat[(i*sizeB)+j] = m_vdStateFreq[m_viSeqB[j-1]] + max(max(Match2Del(i,j),Ins2Del(i,j)),Del2Del(i,j));
//			cout << " D" << flush;
		}
	}

	// Now need to do the traceback.
	// i = seqA, j = seqB, k = state {0=Match;1=Ins;2=Del}
	// 1. Start point
//	cout << "\nTo make start [" << m_vdMatchMat[(sizeA*sizeB)-1] << "," << m_vdInsMat[(sizeA*sizeB)-1] << "," <<  m_vdDelMat[(sizeA*sizeB)-1] << "]";
	k = 0;
	if(m_vdInsMat[(sizeA*sizeB)-1] > m_vdMatchMat[(sizeA*sizeB)-1]) { k = 1;  if(m_vdDelMat[(sizeA*sizeB)-1] > m_vdInsMat[(sizeA*sizeB)-1]) { k = 2; }
	} else { if(m_vdDelMat[(sizeA*sizeB)-1] > m_vdMatchMat[(sizeA*sizeB)-1]) { k = 2; } }
	// Get the maximum log-likelihood
	switch(k) {
	case 0: logL = m_vdMatchMat[(sizeA*sizeB)-1]; break;
	case 1: logL = m_vdInsMat[(sizeA*sizeB)-1]; break;
	case 2: logL = m_vdDelMat[(sizeA*sizeB)-1]; break;
	}


//	cout << "\nStart state: " << k;
	i = sizeA-1; j = sizeB-1;
	while(i>=0 || j>=0) {
		assert(i>= 0 && j >=0);
//		cout << "\n----------------------\nAt ["<<i<<"," << j <<"]:";
		if(i==0 && j == 0) { break; }
		switch(k) {
		case 0:	// Currently match
//			cout << " match " << m_vdMatchMat[(i*sizeB)+j];
			// Build alignment
			assert(i-1 >= 0 && j-1 >= 0);
			m_viAln[0].insert(m_viAln[0].begin(),m_viSeqA[i-1]);
			m_viAln[1].insert(m_viAln[1].begin(),m_viSeqB[j-1]);
			// Check match
			if(fabs( ( Match2Match(i,j) + m_vdPTModel[(m_viSeqA[(i-1)]*m_iChar) + m_viSeqB[j-1]] ) - m_vdMatchMat[(i*sizeB)+j] ) < FLT_EPSILON) {
//				cout << "\n\t---> Was match";
				k = 0; //i--; j--;
			// Check insert
			} else if(fabs( ( Ins2Match(i,j) + m_vdPTModel[(m_viSeqA[(i-1)]*m_iChar) + m_viSeqB[j-1]] ) - m_vdMatchMat[(i*sizeB)+j] ) < FLT_EPSILON) {
//				cout << "\n\t---> Was ins";
				k = 1; //i--;
			// check delete
			} else if(fabs( ( Del2Match(i,j) + m_vdPTModel[(m_viSeqA[(i-1)]*m_iChar) + m_viSeqB[j-1]] ) - m_vdMatchMat[(i*sizeB)+j] ) < FLT_EPSILON) {
//				cout << "\n---> Was del";
				k = 2; //j--;
			} else { Error("BAARP"); }
			if(StorePath) { m_viApproxPath[i] = j; }
			i--; j--;
		break;
		case 1: // Currently ins
//			cout << " ins   " << m_vdInsMat[(i*sizeB)+j];
			// Build alignment
			assert(i-1 >= 0);
			m_viAln[0].insert(m_viAln[0].begin(),m_viSeqA[i-1]);
			m_viAln[1].insert(m_viAln[1].begin(),-1);
			// Check match
			if(fabs( ( Match2Ins(i,j) + m_vdStateFreq[m_viSeqA[i-1]] ) - m_vdInsMat[(i*sizeB)+j] ) < FLT_EPSILON ) {
//				cout << "\n\t---> Was match";
				k = 0; //i--; j--;
			// Check insert
			} else if(fabs( ( Ins2Ins(i,j) + m_vdStateFreq[m_viSeqA[i-1]] ) - m_vdInsMat[(i*sizeB)+j] ) < FLT_EPSILON ) {
//				cout << "\n\t---> Was ins";
				k = 1; //i--;
			} else if(fabs( ( Del2Ins(i,j) + m_vdStateFreq[m_viSeqB[i-1]] ) - m_vdInsMat[(i*sizeB)+j] ) < FLT_EPSILON ) {
				k = 2;
			} else { Error("CAARP"); }
			if(StorePath) { m_viApproxPath[i] = j; }
			i--;
		break;
		case 2: // Currently del
//			cout << " del   " << m_vdDelMat[(i*sizeB)+j];
			// Build alignment
			assert(j-1 >= 0);
			m_viAln[0].insert(m_viAln[0].begin(),-1);
			m_viAln[1].insert(m_viAln[1].begin(),m_viSeqB[j-1]);
			// Check match
			if(fabs( ( Match2Del(i,j) + m_vdStateFreq[m_viSeqB[j-1]] ) - m_vdDelMat[(i*sizeB)+j] ) < FLT_EPSILON ) {
//				cout << "\n\t---> Was match";
				k = 0; //i--; j--;
			// Check insert
			} else if(fabs( ( Ins2Del(i,j) + m_vdStateFreq[m_viSeqB[j-1]] ) - m_vdDelMat[(i*sizeB)+j] ) < FLT_EPSILON ) {
//				cout << "\n\t---> Was insert";
				k = 1; //i--;
			// Check delete
			} else if(fabs( ( Del2Del(i,j) + m_vdStateFreq[m_viSeqB[j-1]] ) - m_vdDelMat[(i*sizeB)+j] ) < FLT_EPSILON ) {
//				cout << "\n\t---> Was delete";
				k = 2; //j--;
			} else { Error("DAARP"); }
			j--;
		break;
		default: Error("BZZZ");
		}
//		cout << "\nNow at ("<<i<<","<<j<<") " << flush;
	}
/*
	// Output the matrices
	cout << "\nSeqA: " << m_viSeqA;
	cout << "\nSeqB: " << m_viSeqB;
	cout << "\n---\nMatches";
	FOR(i,m_viSeqA.size() + 1) {
		cout << "\n";
		FOR(j,m_viSeqB.size() + 1) {
			cout << m_vdMatchMat[(i*(m_viSeqB.size()+1)) + j] << "\t";
		}
	}
	cout << "\n---\nIns";
	FOR(i,m_viSeqA.size() + 1) {
		cout << "\n";
		FOR(j,m_viSeqB.size() + 1) {
			cout << m_vdInsMat[(i*(m_viSeqB.size()+1)) + j] << "\t";
		}
	}
	cout << "\n---\nDel";
	FOR(i,m_viSeqA.size() + 1) {
		cout << "\n";
		FOR(j,m_viSeqB.size() + 1) {
			cout << m_vdDelMat[(i*(m_viSeqB.size()+1)) + j] << "\t";
		}
	}
*/
//	cout << "\nReturning expected alignment: ";
//	cout << "\nSeqA:\t" << Aln[0];
//	cout << "\nSeqB:\t" << Aln[1];

	// Clean up at the end
	m_vdMatchMat.clear();
	m_vdInsMat.clear();
	m_vdDelMat.clear();

	// Return log-likelihood of optimal path
	m_dViterbi_lnL = logL;
//	cout << "\nViterbi (t=" << m_dTime->Val() << " and lnL: " << logL;
	return logL;
}

////////////////////////////////////////////////////////////////////////
// Forward algorithm
// --
// Calculates Prob(A,B|model) <-- The likelihood of the two sequences
double CPairHMM::Forward() {
	int i,j,k;
	int sizeA = m_viSeqA.size()+1, sizeB = m_viSeqB.size()+1;
	CProb logL;

	// Do the approximate calculation if required
	if(m_bDoApprox) { return ApproxForward(); }

	// Check entry conditions
	assert(m_pProbMatchMat == NULL && m_pProbInsMat == NULL && m_pProbDelMat == NULL);	// All probability storage should be empty.
	assert(!m_viSeqA.empty() && !m_viSeqB.empty());
	// Create some space
	int TotalSize = (m_viSeqA.size() + 1) * (m_viSeqB.size() + 1);
	GET_MEM(m_pProbMatchMat,CProb,TotalSize);
	GET_MEM(m_pProbInsMat,CProb,TotalSize);
	GET_MEM(m_pProbDelMat,CProb,TotalSize);

	m_pProbMatchMat[0].Assign(1.0);	// Standard starting condition to begin somewhere.

	// Build the model without logging probabilities
	assert(BuildModel(false));
	// Clarify referencing
	// index i (rows) correspond to sequence A
	// index j (columns) correspond to sequence B
	// Initialise necessary edges
	// 1. Sequence A (inserts before match)
	for(i=1;i<sizeA;i++) {
		m_pProbInsMat[(i*sizeB)] = PrMatch2Ins(i,0);
		m_pProbInsMat[(i*sizeB)].Add(PrIns2Ins(i,0),true);
		m_pProbInsMat[(i*sizeB)].Add(PrDel2Ins(i,0),true);
		m_pProbInsMat[(i*sizeB)].Multiply(m_vdStateFreq[m_viSeqA[i-1]],true);
//		cout << "\n[" << i << "]: " << m_pProbInsMat[(i*sizeB)];
//		m_vdInsMat[(i*sizeB)] = m_vdStateFreq[m_viSeqA[i-1]] + max(Match2Ins(i,0),Ins2Ins(i,0));
	}
	// 2. Sequence B (deletes before match)
	for(j=1;j<sizeB;j++) {
		m_pProbDelMat[j] = PrMatch2Del(0,j);
		m_pProbDelMat[j].Add(PrIns2Del(0,j),true);
		m_pProbDelMat[j].Add(PrDel2Del(0,j),true);
		m_pProbDelMat[j].Multiply(m_vdStateFreq[m_viSeqB[j-1]],true);
//		m_vdDelMat[j] = m_vdStateFreq[m_viSeqB[j-1]] + max(max(Match2Del(0,j),Ins2Del(0,j)),Del2Del(0,j));
	}

	// And now we're ready to go... Forward on
	for(i=1;i<sizeA;i++) {
		for(j=1;j<sizeB;j++) {
			// Do Match states
//			cout << "\nMatch[" << i << "," << j << " == " << (i*sizeB)+j << "]: " << m_pProbMatchMat[(i*sizeB)+j];
			m_pProbMatchMat[(i*sizeB)+j] = PrMatch2Match(i,j);
//			cout << " + " << PrMatch2Match(i,j) << " -> " << m_pProbMatchMat[(i*sizeB)+j];
			m_pProbMatchMat[(i*sizeB)+j].Add(PrIns2Match(i,j),true);
//			cout << " + " << PrIns2Match(i,j) << " -> " << m_pProbMatchMat[(i*sizeB)+j];
			m_pProbMatchMat[(i*sizeB)+j].Add(PrDel2Match(i,j),true);
//			cout << " + " << PrDel2Match(i,j) << " -> " << m_pProbMatchMat[(i*sizeB)+j];
			m_pProbMatchMat[(i*sizeB)+j].Multiply(m_vdPTModel[(m_viSeqA[(i-1)]*m_iChar) + m_viSeqB[j-1]],true);
//			cout << " * " << m_vdPTModel[(m_viSeqA[(i-1)]*m_iChar) + m_viSeqB[j-1]] << " -> " << m_pProbMatchMat[(i*sizeB)+j];
//			m_vdMatchMat[(i*sizeB)+j] = m_vdPTModel[(m_viSeqA[(i-1)]*m_iChar) + m_viSeqB[j-1]] + max(max(Match2Match(i,j),Ins2Match(i,j)),Del2Match(i,j));
			// Do I states (outputs seqA)
			m_pProbInsMat[(i*sizeB)+j] = PrMatch2Ins(i,j);
			m_pProbInsMat[(i*sizeB)+j].Add(PrIns2Ins(i,j),true);
			m_pProbInsMat[(i*sizeB)+j].Add(PrDel2Ins(i,j),true);
			m_pProbInsMat[(i*sizeB)+j].Multiply(m_vdStateFreq[m_viSeqA[i-1]],true);
//			m_vdInsMat[(i*sizeB)+j] = m_vdStateFreq[m_viSeqA[i-1]] + max(Match2Ins(i,j),Ins2Ins(i,j));
			// Do D states (outputs seqB)
			m_pProbDelMat[(i*sizeB)+j] = PrMatch2Del(i,j);
			m_pProbDelMat[(i*sizeB)+j].Add(PrIns2Del(i,j),true);
			m_pProbDelMat[(i*sizeB)+j].Add(PrDel2Del(i,j),true);
			m_pProbDelMat[(i*sizeB)+j].Multiply(m_vdStateFreq[m_viSeqB[j-1]],true);
//			m_vdDelMat[(i*sizeB)+j] = m_vdStateFreq[m_viSeqB[j-1]] + max(max(Match2Del(i,j),Ins2Del(i,j)),Del2Del(i,j));
		}
	}
	logL = m_pProbMatchMat[TotalSize-1];
	logL.Add(m_pProbInsMat[TotalSize-1],true);
	logL.Add(m_pProbDelMat[TotalSize-1],true);
/*
	// Output the matrices
	cout << "\nSeqA: " << m_viSeqA;
	cout << "\nSeqB: " << m_viSeqB;
	cout << "\n---\nMatches";
	FOR(i,m_viSeqA.size() + 1) {
		cout << "\n";
		FOR(j,m_viSeqB.size() + 1) {
			cout << m_pProbMatchMat[(i*(m_viSeqB.size()+1)) + j].LogP() << "\t";
		}
	}
	cout << "\n---\nIns";
	FOR(i,m_viSeqA.size() + 1) {
		cout << "\n";
		FOR(j,m_viSeqB.size() + 1) {
			cout << m_pProbInsMat[(i*(m_viSeqB.size()+1)) + j].LogP() << "\t";
		}
	}
	cout << "\n---\nDel";
	FOR(i,m_viSeqA.size() + 1) {
		cout << "\n";
		FOR(j,m_viSeqB.size() + 1) {
			cout << m_pProbDelMat[(i*(m_viSeqB.size()+1)) + j].LogP() << "\t";
		}
	}
*/

	// Clean up at the end
	DEL_MEM(m_pProbMatchMat);
	DEL_MEM(m_pProbInsMat);
	DEL_MEM(m_pProbDelMat);

	// Return log-likelihood of optimal path
	m_dForward_lnL = logL.LogP();
//	cout << "\nForward (t=" << m_dTime->Val() << "; indel="<<m_dIndelRate->Val() << "; extend=" << m_dProbExtend->Val() << "; FractionInsert: " << m_dFractionInsert->Val() <<") and lnL: " << logL.LogP();
	return logL.LogP();

}
// Builds the path for approximate Forward likelihood computation
bool CPairHMM::BuildApproxPath(double Acc, bool Reset) {
	int i;
	if(!m_viApproxPath.empty()) { if(Reset) { m_viApproxPath.clear(); } else { Error("Called CPairHMM::BuildApproxPath(...) when path already exists..."); } }
	if(!m_viApproxPathSize.empty()) { if(Reset) { m_viApproxPathSize.clear(); } else { Error("Called CPairHMM::BuildApproxPath(...) when bounds already exist..."); } }
	m_viApproxPath.assign(m_viSeqA.size()+1,-1); m_viApproxPathSize.assign(m_viSeqA.size()+1,-1);
	Viterbi(true); m_viApproxPath[0] = 0;
	// Check the path is fully specified (assert)
	FOR(i,(int)m_viApproxPath.size()) {	assert(m_viApproxPath[i] != -1); }
	// Now get the desired degree of accuracy
	ForceAccuracy(Acc);
//	cout << "\nApproxPath: "; FOR(i,(int)m_viApproxPath.size()) { cout << "\n["<<i<< "]: " << m_viApproxPath[i] << " and bound " << m_viApproxPathSize[i]; }
	m_bDoApprox = true;
	return true; // It's worked fine
}
// Performs a few calculations to decide a sensible degree of accuracy
bool CPairHMM::ForceAccuracy(double Acc) {
	int i,counter, step = 10;
	double OrilnL = Forward(), curlnL;
	// Check entry conditions
	assert(!m_viApproxPath.empty() && !m_viApproxPathSize.empty() && m_viApproxPath.size() == m_viApproxPathSize.size());
	// Main loop that builds the approximation
//	cout << "\nBuilding approximation for likelihood " << OrilnL << "\n---" << flush;
	for(counter = m_iApproxSize;counter<m_viSeqB.size()+1;counter += step)	{
		// Build the path
		// Create the bounds. The algorithm will include all paths within m_iApproxSize of the original viterbi alignment.
		FOR(i,(int)m_viApproxPath.size() - 1) {
			assert(m_viApproxPath[i] != -1);
			m_viApproxPathSize[i] = m_viApproxPath[i+1] - m_viApproxPath[i] + counter - 1;
		}
		assert(m_viApproxPath[(int)m_viApproxPath.size()-1] != -1);
		m_viApproxPathSize[(int)m_viApproxPath.size()-1] = m_viApproxPath[(int)m_viApproxPath.size()-1] - m_viApproxPath[(int)m_viApproxPath.size()-2] + counter - 1;
//		cout << "\n\t\t ... built path for counter: " << counter;
		curlnL = ApproxForward(Acc);
//		cout << "\n\tApproxSize = " << counter << " -> " << curlnL << " ; diff = " << fabs(curlnL - OrilnL);
		if(fabs(OrilnL - curlnL) < Acc) { break; }
	}
//	cout << "\nApproxPath: "; FOR(i,(int)m_viApproxPath.size()) { cout << "\n["<<i<< "]: " << m_viApproxPath[i] << " and bound " << m_viApproxPathSize[i]; }
//	exit(-1);
}

// Calculates an approximate value of Prob(A,B|model) <-- The likelihood of the two sequences
// This approximation takes a sample around the viterbi path
double CPairHMM::ApproxForward(double Acc) {
	int i,j,k;
	int sizeA = m_viSeqA.size()+1, sizeB = m_viSeqB.size()+1;
	CProb logL;
	int start = 1, end = sizeB, count = 0;

	// If there's no approximate path then create one
	if(m_viApproxPath.empty()) { BuildApproxPath(Acc,true); }

	// Check entry conditions
	assert(m_pProbMatchMat == NULL && m_pProbInsMat == NULL && m_pProbDelMat == NULL);	// All probability storage should be empty.
	assert(!m_viSeqA.empty() && !m_viSeqB.empty());
	// Create some space
	int TotalSize = (m_viSeqA.size() + 1) * (m_viSeqB.size() + 1);
	GET_MEM(m_pProbMatchMat,CProb,TotalSize);
	GET_MEM(m_pProbInsMat,CProb,TotalSize);
	GET_MEM(m_pProbDelMat,CProb,TotalSize);

	m_pProbMatchMat[0].Assign(1.0);	// Standard starting condition to begin somewhere.

	// Build the model without logging probabilities
	assert(BuildModel(false));
	// Clarify referencing
	// index i (rows) correspond to sequence A
	// index j (columns) correspond to sequence B
	// Initialise necessary edges
	// 1. Sequence A (inserts before match)
	for(i=1;i<sizeA;i++) {
		m_pProbInsMat[(i*sizeB)] = PrMatch2Ins(i,0);
		m_pProbInsMat[(i*sizeB)].Add(PrIns2Ins(i,0),true);
		m_pProbInsMat[(i*sizeB)].Add(PrDel2Ins(i,0),true);
		m_pProbInsMat[(i*sizeB)].Multiply(m_vdStateFreq[m_viSeqA[i-1]],true);
//		cout << "\n[" << i << "]: " << m_pProbInsMat[(i*sizeB)];
//		m_vdInsMat[(i*sizeB)] = m_vdStateFreq[m_viSeqA[i-1]] + max(Match2Ins(i,0),Ins2Ins(i,0));
	}
	// 2. Sequence B (deletes before match)
	for(j=1;j<sizeB;j++) {
		m_pProbDelMat[j] = PrMatch2Del(0,j);
		m_pProbDelMat[j].Add(PrIns2Del(0,j),true);
		m_pProbDelMat[j].Add(PrDel2Del(0,j),true);
		m_pProbDelMat[j].Multiply(m_vdStateFreq[m_viSeqB[j-1]],true);
//		m_vdDelMat[j] = m_vdStateFreq[m_viSeqB[j-1]] + max(max(Match2Del(0,j),Ins2Del(0,j)),Del2Del(0,j));
	}

	// And now we're ready to go... Forward on
	for(i=1;i<sizeA;i++) {
		start = max(0,m_viApproxPath[i] - m_viApproxPathSize[i]);
		end = min(sizeB,m_viApproxPath[i] + m_viApproxPathSize[i]);
		for(j=start;j<end;j++) {
			count ++;
			// Do Match states
//			cout << "\nMatch[" << i << "," << j << " == " << (i*sizeB)+j << "]: " << m_pProbMatchMat[(i*sizeB)+j];
			m_pProbMatchMat[(i*sizeB)+j] = PrMatch2Match(i,j);
//			cout << " + " << PrMatch2Match(i,j) << " -> " << m_pProbMatchMat[(i*sizeB)+j];
			m_pProbMatchMat[(i*sizeB)+j].Add(PrIns2Match(i,j),true);
//			cout << " + " << PrIns2Match(i,j) << " -> " << m_pProbMatchMat[(i*sizeB)+j];
			m_pProbMatchMat[(i*sizeB)+j].Add(PrDel2Match(i,j),true);
//			cout << " + " << PrDel2Match(i,j) << " -> " << m_pProbMatchMat[(i*sizeB)+j];
			m_pProbMatchMat[(i*sizeB)+j].Multiply(m_vdPTModel[(m_viSeqA[(i-1)]*m_iChar) + m_viSeqB[j-1]],true);
//			cout << " * " << m_vdPTModel[(m_viSeqA[(i-1)]*m_iChar) + m_viSeqB[j-1]] << " -> " << m_pProbMatchMat[(i*sizeB)+j];
//			m_vdMatchMat[(i*sizeB)+j] = m_vdPTModel[(m_viSeqA[(i-1)]*m_iChar) + m_viSeqB[j-1]] + max(max(Match2Match(i,j),Ins2Match(i,j)),Del2Match(i,j));
			// Do I states (outputs seqA)
			m_pProbInsMat[(i*sizeB)+j] = PrMatch2Ins(i,j);
			m_pProbInsMat[(i*sizeB)+j].Add(PrIns2Ins(i,j),true);
			m_pProbInsMat[(i*sizeB)+j].Add(PrDel2Ins(i,j),true);
			m_pProbInsMat[(i*sizeB)+j].Multiply(m_vdStateFreq[m_viSeqA[i-1]],true);
//			m_vdInsMat[(i*sizeB)+j] = m_vdStateFreq[m_viSeqA[i-1]] + max(Match2Ins(i,j),Ins2Ins(i,j));
			// Do D states (outputs seqB)
			m_pProbDelMat[(i*sizeB)+j] = PrMatch2Del(i,j);
			m_pProbDelMat[(i*sizeB)+j].Add(PrIns2Del(i,j),true);
			m_pProbDelMat[(i*sizeB)+j].Add(PrDel2Del(i,j),true);
			m_pProbDelMat[(i*sizeB)+j].Multiply(m_vdStateFreq[m_viSeqB[j-1]],true);
//			m_vdDelMat[(i*sizeB)+j] = m_vdStateFreq[m_viSeqB[j-1]] + max(max(Match2Del(i,j),Ins2Del(i,j)),Del2Del(i,j));
		}
	}
	logL = m_pProbMatchMat[TotalSize-1];
	logL.Add(m_pProbInsMat[TotalSize-1],true);
	logL.Add(m_pProbDelMat[TotalSize-1],true);
/*
	// Output the matrices
	cout << "\nSeqA: " << m_viSeqA;
	cout << "\nSeqB: " << m_viSeqB;
	cout << "\n---\nMatches";
	FOR(i,m_viSeqA.size() + 1) {
		cout << "\n";
		FOR(j,m_viSeqB.size() + 1) {
			cout << m_pProbMatchMat[(i*(m_viSeqB.size()+1)) + j].LogP() << "\t";
		}
	}
	cout << "\n---\nIns";
	FOR(i,m_viSeqA.size() + 1) {
		cout << "\n";
		FOR(j,m_viSeqB.size() + 1) {
			cout << m_pProbInsMat[(i*(m_viSeqB.size()+1)) + j].LogP() << "\t";
		}
	}
	cout << "\n---\nDel";
	FOR(i,m_viSeqA.size() + 1) {
		cout << "\n";
		FOR(j,m_viSeqB.size() + 1) {
			cout << m_pProbDelMat[(i*(m_viSeqB.size()+1)) + j].LogP() << "\t";
		}
	}
*/

	// Clean up at the end
	DEL_MEM(m_pProbMatchMat);
	DEL_MEM(m_pProbInsMat);
	DEL_MEM(m_pProbDelMat);

	// Return log-likelihood of optimal path
	m_dForward_lnL = logL.LogP();
//	cout << "\nForward (t=" << m_dTime->Val() << "; indel="<<m_dIndelRate->Val() << "; extend=" << m_dProbExtend->Val() << "; FractionInsert: " << m_dFractionInsert->Val() <<") and lnL: " << logL.LogP();
//	cout << "\nCount == " << count << "\n\n";
	return logL.LogP();

}
// Clean the approximation
void CPairHMM::CleanApprox() {
	m_viApproxPath.clear(); m_viApproxPathSize.clear();
	m_bDoApprox = false;
}
//////////////////////////////////////////////////////////////////
// Alignment output function
double CPairHMM::DoAln(ostream &os) {
	int i,j;
	if(m_viAln.empty()) { Viterbi(); }
	os << "2  " << m_viAln[0].size();
	os << "\n\n" << m_sNameA;
	FOR(i,(int)m_viAln[0].size()) {
		if(i%60 == 0) { os << "\n"; }
		if(m_viAln[0][i] == -1) { os << "-"; continue; }
		assert(InRange(m_viAln[0][i],0,(int)m_sABET.size()));
		os << m_sABET[m_viAln[0][i]];
	}
	os << "\n\n" << m_sNameB;
	FOR(i,(int)m_viAln[1].size()) {
		if(i%60 == 0) { os << "\n"; }
		if(m_viAln[1][i] == -1) { os << "-"; continue; }
		assert(InRange(m_viAln[1][i],0,(int)m_sABET.size()));
		os << m_sABET[m_viAln[1][i]];
	}
	os << "\n\n";

}

//////////////////////////////////////////////////////////////////
// Probability calculations within pairHMM
// --
// A. <<< VITERBI >>>
// All functions compute the transition probability to that state and *NOT* the emission probability
// 1. Going to match; always from (i-1,j-1)
double CPairHMM::Match2Match(int i, int j) {
	return m_vdMatchMat[((i-1)*(m_viSeqB.size()+1))+(j-1)] + m_vdMatchTo[0];
}
double CPairHMM::Ins2Match(int i, int j) {
	return m_vdInsMat[((i-1)*(m_viSeqB.size()+1))+(j-1)] + m_vdInsTo[0];
}
double CPairHMM::Del2Match(int i, int j) {
	return m_vdDelMat[((i-1)*(m_viSeqB.size()+1))+(j-1)] + m_vdDelTo[0];
}
// 2. Going to Ins; always from (i-1,j)
double CPairHMM::Match2Ins(int i, int j) {
	return m_vdMatchMat[((i-1)*(m_viSeqB.size()+1)) + (j)] + m_vdMatchTo[1];
}
double CPairHMM::Ins2Ins(int i, int j) {
	return m_vdInsMat[((i-1)*(m_viSeqB.size()+1)) + (j)] + m_vdInsTo[1];
}
double CPairHMM::Del2Ins(int i, int j) {
	return m_vdDelMat[((i-1)*(m_viSeqB.size()+1)) + (j)] + m_vdDelTo[1];
}
// 3. Going to Del; always from (i,j-1)
double CPairHMM::Match2Del(int i, int j) {
	return m_vdMatchMat[(i*(m_viSeqB.size()+1)) + (j-1)] + m_vdMatchTo[2];
}
double CPairHMM::Del2Del(int i, int j) {
	return m_vdDelMat[(i*(m_viSeqB.size()+1)) + (j-1)] + m_vdDelTo[2];
}
double CPairHMM::Ins2Del(int i, int j) {
	return m_vdInsMat[(i*(m_viSeqB.size()+1)) + (j-1)] + m_vdInsTo[2];
}
// B. <<< FORWARD >>>
// All functions compute the transition probability to that state and *NOT* the emission probability
// 1. Going to match; always from (i-1,j-1)
CProb &CPairHMM::PrMatch2Match(int i, int j) {
	return m_pProbMatchMat[((i-1)*(m_viSeqB.size()+1))+(j-1)].Multiply(m_vdMatchTo[0],false);
}
CProb &CPairHMM::PrIns2Match(int i, int j) {
	return m_pProbInsMat[((i-1)*(m_viSeqB.size()+1))+(j-1)].Multiply(m_vdInsTo[0],false);
}
CProb &CPairHMM::PrDel2Match(int i, int j) {
	return m_pProbDelMat[((i-1)*(m_viSeqB.size()+1))+(j-1)].Multiply(m_vdDelTo[0],false);
}
// 2. Going to Ins; always from (i-1,j)
CProb &CPairHMM::PrMatch2Ins(int i, int j) {
	return m_pProbMatchMat[((i-1)*(m_viSeqB.size()+1)) + (j)].Multiply(m_vdMatchTo[1],false);
}
CProb &CPairHMM::PrIns2Ins(int i, int j) {
	return m_pProbInsMat[((i-1)*(m_viSeqB.size()+1)) + (j)].Multiply(m_vdInsTo[1],false);
}
CProb &CPairHMM::PrDel2Ins(int i, int j) {
	return m_pProbDelMat[((i-1)*(m_viSeqB.size()+1)) + (j)].Multiply(m_vdDelTo[1],false);
}
// 3. Going to Del; always from (i,j-1)
CProb &CPairHMM::PrMatch2Del(int i, int j) {
	return m_pProbMatchMat[(i*(m_viSeqB.size()+1)) + (j-1)].Multiply(m_vdMatchTo[2],false);
}
CProb &CPairHMM::PrDel2Del(int i, int j) {
	return m_pProbDelMat[(i*(m_viSeqB.size()+1)) + (j-1)].Multiply(m_vdDelTo[2],false);
}
CProb &CPairHMM::PrIns2Del(int i, int j) {
	return m_pProbInsMat[(i*(m_viSeqB.size()+1)) + (j-1)].Multiply(m_vdInsTo[2],false);
}


//////////////////////////////////////////////////////////////
// Probably temporary functions
double DumbOptimiser(CPairHMM *Model) {
	vector <double *> vdPars;
	vector <CPar *> vpPars;
	vector <int> OptOrd;
	double OldlnL = -BIG_NUMBER, OptlnL = Model->lnL();
	int i, count = 0;

	// Some initial asserts
	vdPars = Model->m_pModel->GetOptPar(false,false,true,false); // Only matrix parameters
	vdPars.push_back(Model->m_dTime->OptimiserValue());
	vdPars.push_back(Model->m_dProbExtend->OptimiserValue());
	vdPars.push_back(Model->m_dFractionInsert->OptimiserValue());
	vdPars.push_back(Model->m_dIndelRate->OptimiserValue());
	vpPars = Model->m_pModel->m_vpAllOptPar;
	vpPars.push_back(Model->m_dTime);
	vpPars.push_back(Model->m_dProbExtend);
	vpPars.push_back(Model->m_dFractionInsert);
	vpPars.push_back(Model->m_dIndelRate);
	assert(vpPars.size() == vdPars.size());
	FOR(i,(int)vpPars.size()) { OptOrd.push_back(i); }


	// No do the optimise
	cout << "\nThe parameters are: " << vpPars.size() << " pointers and " << vdPars.size() << " doubles...";
	FOR(i,vpPars.size()) {
		cout << "\n---\nvpPars["<<i<<"]: " << *vpPars[i] << flush;
		cout << "\nvdPars["<< i << "]: " << *vdPars[i];
	}

	while(OptlnL - OldlnL > 1.0E-3) {
		OldlnL = OptlnL;
		random_shuffle(OptOrd.begin(),OptOrd.end());
		FOR(i,(int)OptOrd.size()) {
			cout << "\nOptimising: " << *vpPars[OptOrd[i]] << flush;
			OptlnL = GoldenSection(OptlnL,vdPars[OptOrd[i]],vpPars[OptOrd[i]],Model);
			cout << " -> " << *vpPars[OptOrd[i]] << flush;
		}
		cout << "\nOptlnL: " << OptlnL << " cf. " << OldlnL << " = " << OptlnL - OldlnL;
		count ++;
		if(count > 1000) { Error("\nOptimiser hit too many iterations..."); }
	}

	// Clear pointers
	FOR(i,(int)vdPars.size()) { vdPars[i] = NULL; vpPars[i] = NULL; }
	cout << "\nReturning: " << OptlnL;
	return OptlnL;

}

// Functions for outputting surfaces
void CPairHMM::OutputTimeSurface(ostream &os,double low , double high ) {
	double step = (high-low)/20,value=low, oriVal;
	CQPar * ParOut = m_dTime;
	oriVal = ParOut->Val();
	os << "\nOutputting " << *ParOut;
	for(value = low;value<high;value+=step) {
		ParOut->SetVal(value);
		os << "\n" << value << "\t" << Forward();
	}
	ParOut->SetVal(oriVal);
	ParOut = NULL;
}
void CPairHMM::OutputIndelRateSurface(ostream &os,double low , double high ) {
	double step = (high-low)/20,value=low, oriVal;
	CQPar * ParOut = m_dIndelRate;
	oriVal = ParOut->Val();
	os << "\nOutputting " << *ParOut;
	for(value = low;value<high;value+=step) {
		ParOut->SetVal(value);
		os << "\n" << value << "\t" << Forward();
	}
	ParOut->SetVal(oriVal);
	ParOut = NULL;

}
void CPairHMM::OutputFractInsSurface(ostream &os,double low , double high ) {
	double step = (high-low)/20,value=low, oriVal;
	CQPar * ParOut = m_dFractionInsert;
	oriVal = ParOut->Val();
	os << "\nOutputting " << *ParOut;
	for(value = low;value<high;value+=step) {
		ParOut->SetVal(value);
		os << "\n" << value << "\t" << Forward();
	}
	ParOut->SetVal(oriVal);
	ParOut = NULL;

}
void CPairHMM::OutputProbExtendSurface(ostream &os,double low , double high ) {
	double step = (high-low)/20,value=low, oriVal;
	CQPar * ParOut = m_dProbExtend;
	oriVal = ParOut->Val();
	os << "\nOutputting " << *ParOut;
	for(value = low;value<high;value+=step) {
		ParOut->SetVal(value);
		os << "\n" << value << "\t" << Forward();
	}
	ParOut->SetVal(oriVal);
	ParOut = NULL;

}

